package de.gratnik.sensors;

import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.util.Log;

/**
 * Five access to raw sensors data and to data after DSP
 * 
 * @author rembo
 * 
 */
public class Sensors implements SensorEventListener {

	private final static boolean DEBUG = true;

	private final static String TAG = "Sensors";
	
	SensorEventListener listener = null;

	private SensorManager sm = null;

	private Sensor Accelerometer = null;

	private Sensor LinearAcceleration = null;

	private Sensor Gravity = null;

	private Sensor Gyroscope = null;

	private Sensor Rotation = null;

	private Sensor Orientation = null;

	public static class SensorValue {
		public long timestapm = 0;
		public float X = 0;
		public float Y = 0;
		public float Z = 0;
	}
	
	private static final int dataSize = 1000;
	private SensorValue[] linearAcceleremoterValues = new SensorValue[dataSize];
	private SensorValue[] linearAcceleremoterValuesDSP = new SensorValue[dataSize];
	private int laPointer = 999;
	

	public Sensors() {
		
		for (int i =0; i < dataSize; ++i) {
			linearAcceleremoterValues[i] = new SensorValue();
			linearAcceleremoterValuesDSP[i] = new SensorValue();
			
		}

	}

	public void Init(Activity activity, SensorEventListener listener) {
		if (DEBUG)
			Log.i(TAG, "Sensors Init() for Activity " + activity.toString());
		
		this.listener =listener;

		sm = (SensorManager) activity.getSystemService(activity.SENSOR_SERVICE);

		Accelerometer = sm.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		LinearAcceleration = sm
				.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION);
		Gravity = sm.getDefaultSensor(Sensor.TYPE_GRAVITY);
		Gyroscope = sm.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
		Rotation = sm.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
		Orientation = sm.getDefaultSensor(Sensor.TYPE_ORIENTATION);
		
		sm.registerListener(this, LinearAcceleration,
				SensorManager.SENSOR_DELAY_FASTEST);
		sm.registerListener(this, Orientation, SensorManager.SENSOR_DELAY_FASTEST);
		
		sm.registerListener(this, Accelerometer, SensorManager.SENSOR_DELAY_FASTEST);		
		sm.registerListener(this, Gravity, SensorManager.SENSOR_DELAY_FASTEST);
		sm.registerListener(this, Gyroscope, SensorManager.SENSOR_DELAY_FASTEST);
		sm.registerListener(this, Rotation, SensorManager.SENSOR_DELAY_FASTEST);
		
	}

	public void onAccuracyChanged(Sensor sensor, int accuracy) {
		// TODO Auto-generated method stub

	}

	public void onSensorChanged(SensorEvent e) {
		
		if (e.sensor.getType() == Sensor.TYPE_LINEAR_ACCELERATION) {
			
			
			
			laPointer++;
			if (laPointer == dataSize) {
				laPointer = 0;
			}
			
			SensorValue value = linearAcceleremoterValues[laPointer];
			value.timestapm = e.timestamp;
			value.X = e.values[0];
			value.Y = e.values[1];
			value.Z = e.values[2];
			
			DSPfilterType1(linearAcceleremoterValues, linearAcceleremoterValuesDSP, laPointer, dataSize);
			
			e.values[0] = linearAcceleremoterValuesDSP[laPointer].X;
			e.values[1] = linearAcceleremoterValuesDSP[laPointer].Y;
			e.values[2] = linearAcceleremoterValuesDSP[laPointer].Z;
			
			
			
		}
		
		listener.onSensorChanged(e);

	}

	public Sensor getAccelerometer() {
		return Accelerometer;
	}

	public void setAccelerometer(Sensor accelerometer) {
		Accelerometer = accelerometer;
	}

	public Sensor getLinearAcceleration() {
		return LinearAcceleration;
	}

	public void setLinearAcceleration(Sensor linearAcceleration) {
		LinearAcceleration = linearAcceleration;
	}

	public Sensor getGravity() {
		return Gravity;
	}

	public void setGravity(Sensor gravity) {
		Gravity = gravity;
	}

	public Sensor getGyroscope() {
		return Gyroscope;
	}

	public void setGyroscope(Sensor gyroscope) {
		Gyroscope = gyroscope;
	}

	public Sensor getRotation() {
		return Rotation;
	}

	public void setRotation(Sensor rotation) {
		Rotation = rotation;
	}

	public Sensor getOrientation() {
		return Orientation;
	}

	public void setOrientation(Sensor orientation) {
		Orientation = orientation;
	}
	
	private static void DSPfilterType1(SensorValue[] raw, SensorValue[] result, int point, int dataSize) {
		float K0 = (float) 1.0;
		float K1 = (float) 0.75;
		float K2 = (float) 0.5;
		float SK = K0+K1+K2;
		
		for (int i = 0; i < dataSize; ++i) {
			int i0 = i + point;
			if (i0 >= dataSize) {
				i0 = i0 - dataSize;
			}
			
			int i1 = i0 -1;
			if (i1 < 0) {
				i1 = dataSize + i1;				
			}
			int i2 = i0 - 2;
			if (i2 < 0) {
				i2 = dataSize + i2;				
			}
			//Log.i(TAG, i0 + " " + i1 + " " + i2);
			result[i0].timestapm = raw[i0].timestapm;
			result[i0].X = (K0*th(raw[i0].X) + K1*th(raw[i1].X)+K2*th(raw[i2].X))/SK;
			result[i0].Y = (K0*th(raw[i0].Y) + K1*th(raw[i1].Y)+K2*th(raw[i2].Y))/SK;
			result[i0].Z = (K0*th(raw[i0].Z) + K1*th(raw[i1].Z)+K2*th(raw[i2].Z))/SK;
			
			
			
		}
	}
	
	private static float th(float value) {
		float th = (float) 0.2;
		if (Math.abs(value) > th) {
			return value;
		}
		return 0;
	}
}
